#include "CAN_receive.h"
#include "stdlib.h"
#include "math.h"
#include "main.h"
#include "string.h"

extern FDCAN_HandleTypeDef hfdcan1;

#define get_dm_motor_measure(ptr, data)                     \
{                                                           \
    (ptr)->last_p_int = (ptr)->p_int;                       \
    (ptr)->last_ecd = (ptr)->ecd;                           \
    (ptr)->id = (data)[0] & 0x0F;                           \
    (ptr)->err = (data)[0] >> 4;                            \
    (ptr)->p_int = ((data)[1] << 8 | (data)[2]);            \
    (ptr)->v_int = ((data)[3] << 4 | (data)[4] >> 4);       \
    (ptr)->t_int = (((data)[4] & 0xF) << 8 | (data)[5]);    \
    (ptr)->T_MOS = (float)(data[6]);                        \
    (ptr)->T_Rotor = (float)(data[7]);                      \
    (ptr)->ecd = (ptr)->p_int >> 3;                         \
}

//用于存放CAN总线电机反馈数据的数组
dm_motor_measure_t dm_motor_data[4];		//FDCAN1

static FDCAN_TxHeaderTypeDef dm_tx_message;
static uint8_t 				 dm_can_send_data[8];

/**
 * @brief  uint转float
 * @param  x_int   最大值
 * @param  x_min   最小值
 * @param  bits    位数
 */
float uint_to_float(int x_int, float x_min, float x_max, int bits)
{
  float span = x_max - x_min;
  float offset = x_min;
  return ((float)x_int)*span/((float)((1<<bits)-1)) + offset;
}

/**
 * @brief  float转uint
 * @param  x_int   最大值
 * @param  x_min   最小值
 * @param  bits    位数
 */
int float_to_uint(float x, float x_min, float x_max, int bits)
{ 
  float span = x_max - x_min;
  float offset = x_min;
  return (int) ((x-offset)*((float)((1<<bits)-1))/span);
}

 /**
   * @brief          hal库CAN回调函数,接收电机数据
   * @param[in]      hcan:CAN句柄指针
   * @retval         none
   */
 void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo0ITs)
 {
  	FDCAN_RxHeaderTypeDef rx_header;
   	uint8_t rx_data[64];

    if((RxFifo0ITs & FDCAN_IT_RX_FIFO0_NEW_MESSAGE) != RESET)
    {
        HAL_FDCAN_GetRxMessage(hfdcan, FDCAN_RX_FIFO0, &rx_header, rx_data);
        
        switch (rx_header.Identifier)
        {
            case CAN_DM1_MASTER_ID:
            {
                get_dm_motor_measure(&dm_motor_data[0], rx_data);
                dm_motor_data[0].p_fp32 = uint_to_float(dm_motor_data[0].p_int, P_MIN, P_MAX, 16);
                dm_motor_data[0].v_fp32 = uint_to_float(dm_motor_data[0].v_int, V_MIN, V_MAX, 12);
                dm_motor_data[0].t_fp32 = uint_to_float(dm_motor_data[0].t_int, T_MIN_4310, T_MAX_4310, 12);            
                break;
            }
            default:
            {
                break;
            }
        }
    }
 }

/**
 * @brief 		电机出现过热等错误时，发送“清除”命令可以清除错误。
 * @param[in] can id 
 * @retval 		none
 */
void CAN_cmd_DMmotor_clear_errors(uint16_t id)
{
    dm_tx_message.Identifier = id;
    dm_tx_message.IdType = FDCAN_STANDARD_ID;
    dm_tx_message.TxFrameType = FDCAN_DATA_FRAME;
    dm_tx_message.DataLength = 0x08;
    dm_can_send_data[0] = 0xFF;
    dm_can_send_data[1] = 0xFF;
    dm_can_send_data[2] = 0xFF;
    dm_can_send_data[3] = 0xFF;
    dm_can_send_data[4] = 0xFF;
    dm_can_send_data[5] = 0xFF;
    dm_can_send_data[6] = 0xFF;
    dm_can_send_data[7] = 0xFB;
    HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan1, &dm_tx_message, dm_can_send_data);
}

/**
 * @brief		使能 达妙电机
 * @param[in] 	id：Slave id 
 * @retval 		none
 */
void CAN_cmd_DMmotor_enable(uint16_t id)
{
	dm_tx_message.Identifier = id;
    dm_tx_message.IdType = FDCAN_STANDARD_ID;
    dm_tx_message.TxFrameType = FDCAN_DATA_FRAME;
    dm_tx_message.DataLength = 0x08;
	dm_tx_message.ErrorStateIndicator =  FDCAN_ESI_ACTIVE;
	dm_tx_message.BitRateSwitch = FDCAN_BRS_OFF;//
	dm_tx_message.FDFormat =  FDCAN_CLASSIC_CAN;            // CANFD
	dm_tx_message.TxEventFifoControl =  FDCAN_NO_TX_EVENTS;  
	dm_tx_message.MessageMarker = 0;
	
	dm_can_send_data[0] = 0xFF;
	dm_can_send_data[1] = 0xFF;
	dm_can_send_data[2] = 0xFF;
	dm_can_send_data[3] = 0xFF;
	dm_can_send_data[4] = 0xFF;
	dm_can_send_data[5] = 0xFF;
	dm_can_send_data[6] = 0xFF;
	dm_can_send_data[7] = 0xFC;
	
    HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan1, &dm_tx_message, dm_can_send_data);
}

/**
 * @brief 		失能 达妙电机
 * @param[in] 	id：Slave id 
 * @retval 		none
 */
void CAN_cmd_DMmotor_failure(uint16_t id)
{
	dm_tx_message.Identifier = id;
    dm_tx_message.IdType = FDCAN_STANDARD_ID;
    dm_tx_message.TxFrameType = FDCAN_DATA_FRAME;
    dm_tx_message.DataLength = 0x08;
	dm_can_send_data[0] = 0xFF;
	dm_can_send_data[1] = 0xFF;
	dm_can_send_data[2] = 0xFF;
	dm_can_send_data[3] = 0xFF;
	dm_can_send_data[4] = 0xFF;
	dm_can_send_data[5] = 0xFF;
	dm_can_send_data[6] = 0xFF;
	dm_can_send_data[7] = 0xFD;
    HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan1, &dm_tx_message, dm_can_send_data);
}

/**
 * @brief		保存 达妙电机 零点
 * @param[in] 	id：Slave id 
 * @retval 		none
 */
void CAN_cmd_DMmotor_save_zero(uint16_t id)
{
	dm_tx_message.Identifier = id;
    dm_tx_message.IdType = FDCAN_STANDARD_ID;
    dm_tx_message.TxFrameType = FDCAN_DATA_FRAME;
    dm_tx_message.DataLength = 0x08;
	dm_can_send_data[0] = 0xFF;
	dm_can_send_data[1] = 0xFF;
	dm_can_send_data[2] = 0xFF;
	dm_can_send_data[3] = 0xFF;
	dm_can_send_data[4] = 0xFF;
	dm_can_send_data[5] = 0xFF;
	dm_can_send_data[6] = 0xFF;
	dm_can_send_data[7] = 0xFE;
    HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan1, &dm_tx_message, dm_can_send_data);
}

/**
 * @brief		达妙电机 位置MIT模式 发送函数
 * @param[in] 	id：Slave id
 * @param[in] 	_pos：位置
 * @param[in] 	_vel：速度
 * @param[in] 	_KP：位置比例系数
 * @param[in] 	_KD：位置微分系数
 * @param[in] 	_torq：转矩
 * @retval 		none
 */
void CAN_cmd_DMmotor4310_MIT(uint16_t id, float _pos, float _vel, float _KP, float _KD, float _torq)
{
	uint16_t pos_tmp,vel_tmp,kp_tmp,kd_tmp,tor_tmp;
	pos_tmp = float_to_uint(_pos, P_MIN, P_MAX, 16);
	vel_tmp = float_to_uint(_vel, V_MIN, V_MAX, 12);
	kp_tmp = float_to_uint(_KP, KP_MIN, KP_MAX, 12);
	kd_tmp = float_to_uint(_KD, KD_MIN, KD_MAX, 12);
	tor_tmp = float_to_uint(_torq, T_MIN_4310, T_MAX_4310, 12);

	dm_tx_message.Identifier = id;
    dm_tx_message.IdType = FDCAN_STANDARD_ID;
    dm_tx_message.TxFrameType = FDCAN_DATA_FRAME;
    dm_tx_message.DataLength = 0x08;
	dm_can_send_data[0] = (pos_tmp >> 8);
	dm_can_send_data[1] = pos_tmp;
	dm_can_send_data[2] = (vel_tmp >> 4);
	dm_can_send_data[3] = ((vel_tmp&0xF)<<4)|(kp_tmp>>8);
	dm_can_send_data[4] = kp_tmp;
	dm_can_send_data[5] = (kd_tmp >> 4);
	dm_can_send_data[6] = ((kd_tmp&0xF)<<4)|(tor_tmp>>8);
	dm_can_send_data[7] = tor_tmp;
    HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan1, &dm_tx_message, dm_can_send_data);
}	

/**
 * @brief		达妙电机 位置速度模式 发送函数
 * @param[in] 	id：Slave id
 * @param[in] 	_pos：位置
 * @param[in] 	_vel：速度
 * @retval 		none
 */
void CAN_cmd_DMmotor_speed_pos(uint16_t id, float _pos, float _vel)
{
	uint8_t *pbuf,*vbuf;
	pbuf=(uint8_t*)&_pos;
	vbuf=(uint8_t*)&_vel;
	
	dm_tx_message.Identifier = id;
    dm_tx_message.IdType = FDCAN_STANDARD_ID;
    dm_tx_message.TxFrameType = FDCAN_DATA_FRAME;
    dm_tx_message.DataLength = 0x08;
	dm_can_send_data[0] = *pbuf;
	dm_can_send_data[1] = *(pbuf+1);
	dm_can_send_data[2] = *(pbuf+2);
	dm_can_send_data[3] = *(pbuf+3);
	dm_can_send_data[4] = *vbuf;
	dm_can_send_data[5] = *(vbuf+1);
	dm_can_send_data[6] = *(vbuf+2);
	dm_can_send_data[7] = *(vbuf+3);
    HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan1, &dm_tx_message, dm_can_send_data);
}	

/**
 * @brief		达妙电机 速度模式 发送函数
 * @param[in] 	id：Slave id
 * @param[in] 	_vel：速度
 * @retval 		none
 */
void CAN_cmd_DMmotor_speed(uint16_t id, float _vel)
{
	uint8_t *vbuf;
	vbuf=(uint8_t*)&_vel;

	dm_tx_message.Identifier = id;
    dm_tx_message.IdType = FDCAN_STANDARD_ID;
    dm_tx_message.TxFrameType = FDCAN_DATA_FRAME;
	dm_tx_message.DataLength = 0x04;
	dm_can_send_data[0] = *vbuf;
	dm_can_send_data[1] = *(vbuf+1);
	dm_can_send_data[2] = *(vbuf+2);
	dm_can_send_data[3] = *(vbuf+3);
    HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan1, &dm_tx_message, dm_can_send_data);
}

/******************* FDCAN1 ******************/
/**
 * @brief          返回第一个达妙电机J4310电机数据指针
 * @param[in]      none
 * @retval         电机数据指针
*/
const dm_motor_measure_t *get_dm_motor_1_measure_point(void)
{
	return &dm_motor_data[0];
}


